#!/bin/python
import cv2
import numpy as np
import time
import serial
import struct
import signal
import sys
def img_resize(image,x=480,y=48000):    #重新设定图像
    """
    :param image:   需处理图像
    :param x:   X像素
    :param y:   Y像素
    :return:    处理后的图像
    """
    height, width = image.shape[0], image.shape[1]
    # 设置新的图片分辨率框架
    width_new = x
    height_new = y
    # 判断图片的长宽比率
    if width / height >= width_new / height_new:
        img_new = cv2.resize(image, (width_new, int(height * width_new / width)))
    else:
        img_new = cv2.resize(image, (int(width * height_new / height), height_new))
    return img_new
def mask(img,min,max=255,pattern = 1):
    """
    上下阈值化
    :param img:     需处理的图像
    :param min:     下阈值
    :param max:     上阈值
    :param pattern: 0：阈值之内为黑色    1（默认）：阈值之内为白色
    :return:    处理后的图像
    """
    mask = np.zeros_like(img)
    if pattern == 1:
        mask[(img>=min) & (img<=max)] = 255
    elif pattern == 0:
        mask[:] = 255
        mask[(img >= min) & (img <= max)] = 0
    return mask
def Contours(name,img):
    """
    寻找并画轮廓
    :param name:    显示时的图像的名称
    :param img:     需处理的图像
    :return:        轮廓的列表
    """
    contours, _ = cv2.findContours(img, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    init_np = np.zeros((img.shape[0], img.shape[1],3), dtype=np.uint8)
    #init_np[:] = 255
    cv2.drawContours(init_np, contours, -1, (0,0,255), 1)  # 最后一个参数表示轮廓线的宽度，-1表示填充轮廓
    # 显示结果
    cv2.imshow(name, init_np)
    return contours
def erod(name,img,kernel_size,interations = 1,display = 0):
    """
    腐蚀
    :param name:     显示时的图像的名称
    :param img:      需处理的图像
    :param kernel_size: 核的大小（长宽一致）
    :param interations: 处理次数（默认为1）
    :param display:     （1：是；0：否）显示
    :return: 处理后的图像
    """
    kernel = np.ones((kernel_size, kernel_size), np.uint8)
    erod_img =  cv2.erode(img, kernel, iterations=interations)
    if display:
        cv2.imshow(name, erod_img)
    return erod_img
def dilate(name,img,kernel_size,interations = 1):
    """
    膨胀
    :param name:     显示时的图像的名称
    :param img:      需处理的图像
    :param kernel_size: 核的大小（长宽一致）
    :param interations: 处理次数（默认为1）
    :return: 处理后的图像
    """
    kernel = np.ones((kernel_size, kernel_size), np.uint8)
    dilated_img =  cv2.dilate(img, kernel, iterations=interations)
    cv2.imshow(name, dilated_img)
    return dilated_img
def Hexagon_Contours(img):  #匹配第一关六边形轮廓
    """
    匹配第一关六边形轮廓
    :param img: 需处理的图像
    :return: 六边形contours,中心坐标
    """
    Hexagon_Contours_List = []
    contours, _ = cv2.findContours(img, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    #print(type([contours[0]]))
    # 遍历所有轮廓
    for contour in contours:
        # 使用多边形逼近将轮廓近似为多边形
        archlength = cv2.arcLength(contour, True)
        epsilon = 0.04 * archlength
        approx = cv2.approxPolyDP(contour, epsilon, True)
        # 如果近似多边形有6个顶点，则可能是正六边形
        if len(approx) == 6 and archlength > 100:
            # 进一步检查是否为正六边形
            # 计算每条边的长度
            side_lengths = []
            for i in range(6):
                side_length = np.linalg.norm(approx[i][0] - approx[(i + 1) % 6][0])
                side_lengths.append(side_length)
            # 计算边长的标准差
            std_dev = np.std(side_lengths)

            # 如果边长的标准差很小，则认为是正六边形
            if std_dev < 10:  # 这个值可以根据需要调整
                (x, y), radius = cv2.minEnclosingCircle(contour)
                center = (int(x), int(y))
                #print(type(center))
                radius = int(radius)
                minRadius = radius
                for i in contour:
                    dist = int(np.linalg.norm(i - center))
                    if dist < minRadius:
                        minRadius = dist
                if minRadius/radius > 0.6:
                    Hexagon_Contours_List.append([contour,center])
                    #print(contour)
                    #print(f"外圈半径{radius}\n内圈半径{minRadius}\n占比{minRadius / radius}\n")
    Hexagon_Contours_List.sort(key = lambda x: x[1][1])
    return Hexagon_Contours_List

def Part_1(img_raw,debug = 0):   #第一关
    """
    第一关识别函数
    :param img_raw: 待处理原图
    :param debug:   默认0：非调试模式
    :return: (1|0 , [( 1|0 , 中心坐标),( 1|0 , 中心坐标),( 1|0 , 中心坐标)])    1:成功（雄蕊）； 0：失败（雌蕊）
    """
    Result_List = []

    img_hsv = cv2.cvtColor(img_raw, cv2.COLOR_BGR2HSV)
    img_h, img_s, img_v = cv2.split(img_hsv)
    mask_img_h = mask(img_h, 59, 123, 0)
    mask_img_s = mask(img_s, 35, 255, 0)
    mask_img = cv2.bitwise_or(mask_img_h, mask_img_s)
    L = Hexagon_Contours(mask_img)
    if len(L) == 3:
        img_roi_mask = np.zeros_like(mask_img)
        cv2.drawContours(img_roi_mask, [x[0] for x in L], -1, 255, -1)
        img_grey = cv2.cvtColor(img_raw, cv2.COLOR_BGR2GRAY)
        if debug:
            img_result = np.zeros_like(img_grey)
        for i in L:
            img_cnt = np.zeros_like(mask_img)
            cv2.drawContours(img_cnt, [i[0]], -1, 255, -1)
            img_roi_part_array = img_grey[img_cnt == 255]
            thresh_value,_ = cv2.threshold(img_roi_part_array, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
            if debug:
                img_grey_part = cv2.bitwise_and(img_grey, img_cnt)
                _, part_thresh = cv2.threshold(img_grey_part, thresh_value, 255, cv2.THRESH_BINARY)
                img_result += part_thresh
            if img_grey[i[1][1],i[1][0]] > thresh_value :
                print("雌蕊",i[1])
                Result_List.append((b'\x11',i[1]))
            else:
                print("雄蕊",i[1])
                Result_List.append((b'\x12', i[1]))



        if debug:
            img_result = cv2.cvtColor(img_result, cv2.COLOR_GRAY2BGR)
            cv2.drawContours(img_result, [x[0] for x in L], -1, (255,0,0), 1)
            for i in L:
                cv2.circle(img_result, i[1],1, (0,0,255), 1)
            cv2.imshow('img_raw', img_raw)
            cv2.imshow('img_result', img_result)
            #cv2.imshow('img_roi_h_thresh', img_roi_h_thresh)
        return (1,Result_List)
    else:
        #cv2.imshow('img_raw', img_raw)
        if debug:
            img_roi_mask = np.ones_like(mask_img)
            cv2.drawContours(img_roi_mask, [x[0] for x in L], -1, (255, 255, 0), -1)
            img_roi_mask_3 = cv2.merge([img_roi_mask, img_roi_mask, img_roi_mask])
            img_roi = cv2.bitwise_and(img_roi_mask_3, img_raw)
            #cv2.imshow('img_raw_error', img_raw)
            cv2.imshow("first_filter",mask_img)
            cv2.imshow('img_roi_error', img_roi)

        return (0,Result_List)

def ganshe():
    global send_data
    try:
        zhonglei = input("c:雌，x:雄（,分隔）:").split(",")
        try:
            x = [int(i) for i in input("三个坐标：（,分隔）：").split(",")]
        except:
            x = [0,0,0]
        x_1 = struct.pack('b',x[0])
        x_2 = struct.pack('b',x[1])
        x_3 = struct.pack('b',x[2])
        send_data = char2b[zhonglei[0]] + char2b[zhonglei[1]] + char2b[zhonglei[2]] + x_1 + x_2 + x_3
        print(send_data)
        if(input("发送？（y/n）:") == 'n'):
            ganshe()
    except:
        print("输入失败")
        ganshe()
def signal_handler(sig, frame):
    global cap
    print("捕获到暂停信号，释放资源")
    if cap:
        cap.release()
        cv2.destroyAllWindows()
    sys.exit(0)


char2b = {"c":b'\x11',"x":b'\x12'}
b2char = {b'\x11':"雌",b'\x12':"雄"}
error_num = 0
error_num_shibie = 0
cnt = []
send_data = ''
cap = None
# 注册信号处理器
signal.signal(signal.SIGTSTP, signal_handler)
signal.signal(signal.SIGINT, signal_handler)

try:
    ser = serial.Serial(port="/dev/ttyAMA0",baudrate=9600)
    print("串口打开成功")
except serial.SerialException as e:
    print(f"串口打开失败: {e}")

cap = cv2.VideoCapture(0)
if cap.isOpened():
    print("摄像头打开成功")
else:
    print("摄像头打开失败")
cv2.namedWindow("Raw_Img", cv2.WINDOW_NORMAL)
ret, frame = cap.read()
while(ret != True and error_num <=100):
    ret, frame = cap.read()
    error_num+=1
if ret == True:
    img_raw = frame
    #img_raw = img_resize(img_raw, 360)
    cv2.imshow("Raw_Img", img_raw)
cv2.waitKey(1)  # Esc退出
    
if(input("发送启动信号？（y/n）")=='y'):
    ser.write(b"\x66")
while 1:
    error_num = 0
    error_num_shibie = 0
    x_1 = b''
    x_2 = b''
    x_3 = b''
    send_data = b''
    cnt.clear()
    data = ser.read(1)
    if data == b'\x01':
        print("第一关")
        iret, frame = cap.read()
        while(ret != True and error_num <=100):
            ret, frame = cap.read()
            error_num+=1
        if ret == True:
            img_raw = frame
            img_raw = img_resize(img_raw, 360)
            cv2.imshow("Raw_Img", img_raw)
            cv2.waitKey(1)
            st, cnt = Part_1(img_raw)
            while(st == 0 and error_num_shibie<= 50):
                st, cnt = Part_1(img_raw)
                error_num_shibie+=1
        else:
            print("照片读取失败")
        if st:
            x_1 = struct.pack('b',0)
            x_2 = struct.pack('b',0)
            x_3 = struct.pack('b',0)
            send_data = cnt[0][0]+cnt[1][0]+cnt[2][0]+x_1+x_2+x_3
            print(f"{b2char[cnt[0][0]]},{b2char[cnt[1][0]]},{b2char[cnt[2][0]]}send_data:{send_data}")
        else:
            print("未识别到")
        yn = input("干涉？（y/n）:")
        while (yn != 'y' and yn != 'n'):
            yn = input("y/n?")
        if 1:
            ganshe()

    elif data == b'\x02':
        print("第二关")
        ret, frame = cap.read()
        while (ret != True and error_num <= 100):
            ret, frame = cap.read()
            error_num += 1
        if ret == True:
            img_raw = frame
            img_raw = img_resize(img_raw, 360)
            cv2.imshow("Raw_Img",img_raw)
            cv2.waitKey(1)
        else:
            print("照片读取失败")

        ganshe()
    elif data == b'\x03':
        print('第三关')
        ret, frame = cap.read()
        while (ret != True and error_num <= 100):
            ret, frame = cap.read()
            error_num += 1
        if ret == True:
            img_raw = frame
            img_raw = img_resize(img_raw, 360)
            cv2.imshow("Raw_Img",img_raw)
            cv2.waitKey(1)
        else:
            print("照片读取失败")
        ganshe()
    ser.write(send_data)
    if cv2.waitKey(1) == 27:  # Esc退出
        break
    ser.flushInput()
